
#include "common_def.h"
#include "soc_osal.h"
#include "securec.h"
#include "product.h"
#include "bts_le_gap.h"
#include "uart.h"
#include "pinctrl.h"
#include "sle_device_discovery.h"
#include "sle_connection_manager.h"
#include "sle_client.h"
#include "app_init.h"
#include "sle_common.h"//测距
#include <math.h>

int8_t row,distance,speeed;
int8_t leida_data[6]={0};



int rssi_distance;//距离
int n=2;  //环境衰减因子
int A=60; //1米时的信号强度

int rssi_sum;
int num;
int rssi_average;


#define RECV_PKT_CNT 3

uint8_t LOCK_state;//0是上锁。1是解锁
uint8_t Sle_Anounce_Flag;  //通过判断解析后的广播数据有没有指定数据，有的话将该位置对应值，在主函数执行相关操作。






#define SLE_UART_CLIENT_MAX_CON         2
uint16_t g_sle_con_id[SLE_UART_CLIENT_MAX_CON] = { 0 };//g_sle_con_id连接id的生成依靠的是服务端与客户端的连接顺序，从0开始计数。比如第一个服务端与客户端连接，则它的连接id为0
uint16_t g_sle_uart_conn_num = 0;

#define MAX_DATA_SIZE 1024
char data_buffer[MAX_DATA_SIZE];
int flag;

#define SLE_MTU_SIZE_DEFAULT 512
#define SLE_SEEK_INTERVAL_DEFAULT 100
#define SLE_SEEK_WINDOW_DEFAULT 100
#define UUID_16BIT_LEN 2
#define UUID_128BIT_LEN 16

#define SLE_SERVER_NAME "sle_test"


static ssapc_find_service_result_t g_sle_find_service_result = {0};
static sle_announce_seek_callbacks_t g_sle_uart_seek_cbk = {0};
static sle_connection_callbacks_t g_sle_uart_connect_cbk = {0};
static ssapc_callbacks_t g_sle_uart_ssapc_cbk = {0};
static sle_addr_t g_sle_remote_addr = {0};
ssapc_write_param_t g_sle_send_param = {0};
//uint16_t g_sle_uart_conn_id = 0;
/* 开启扫描 */
void sle_start_scan(void)
{
    sle_seek_param_t param = {0};
    param.own_addr_type = 0;
    param.filter_duplicates = 0;
    param.seek_filter_policy = 0;
    param.seek_phys = 1;
    param.seek_type[0] = 1;
    param.seek_interval[0] = SLE_SEEK_INTERVAL_DEFAULT;
    param.seek_window[0] = SLE_SEEK_WINDOW_DEFAULT;
    sle_set_seek_param(&param);
    sle_start_seek();
}

/* 星闪协议栈使能回调 */
/*在回调函数中，运行sle_start_scan();触发扫描使能回调函数sle_client_seek_enable_cbk*/
static void sle_client_sle_enable_cbk(errcode_t status)
{
    unused(status);
    printf("sle enable.\r\n");
    sle_start_scan();//触发扫描使能回调函数
}

/* 扫描使能回调函数 */
/*扫描完成后触发扫描结果回调函数*/
static void sle_client_seek_enable_cbk(errcode_t status)
{
    if (status != 0) {
        printf("[%s] status error\r\n", __FUNCTION__);
    }
}




/* 返回扫描结果回调 */
/*扫描完成后，停止扫描，触发扫描关闭回调函数*/
static void sle_client_seek_result_info_cbk(sle_seek_result_info_t *seek_result_data)
{
    printf("[seek_result_info_cbk] scan data : ");
    osal_printk("sle_sample_seek_result_info_cbk  [%02x,%02x,%02x,%02x,%02x,%02x]\n", 
                seek_result_data->addr.addr[0], 
                seek_result_data->addr.addr[1],
                seek_result_data->addr.addr[2],
                seek_result_data->addr.addr[3],
                seek_result_data->addr.addr[4],
                seek_result_data->addr.addr[5]);
    
    if (!seek_result_data) {
        printf("status error\r\n");
        return;
    }
     /*信号衰减遵循对数距离路径损耗模型*/
    /*
     RSSI(d)=P_r(d_0)−10n*lg(d/d0)+X0

     RSSI(d)：距离 d 处的信号强度（dBm），​负值**​（如-70dBm表示信号较弱）
     P_r(d_0)**：参考距离 d_0（通常取1米）处的信号强度（dBm）
     n：路径损耗指数，​取决于环境**​（自由空间≈2，办公室≈3-4，复杂室内≈4-6）
     X_\sigma**：高斯随机噪声（均值为0，标准差σ），反映多径效应、障碍物干扰

     关键公式变形（距离计算）公式为​：
     d=10^((abs(RSSI)-A)/(10*n))
     
    */

    rssi_sum=(int8_t)seek_result_data->rssi+rssi_sum;
    num++;
    if(num==RECV_PKT_CNT)
    {
        rssi_average=rssi_sum/num;
    }

    // 解析自定义广播数据：解析广播数据中的厂商自定义字段（0xFF类型）
    uint8_t *data = seek_result_data->data;
    uint16_t idx = 0;
    while (idx < seek_result_data->data_length) {
        uint8_t len = data[idx++];        // AD结构长度
        if (len == 0) break;
        
        uint8_t type = data[idx++];       // 数据类型
        uint8_t *payload = &data[idx];    // 数据载荷
        
        // 识别厂商自定义数据（0xFF类型）
        if (type == 0xFF) {
            // 验证厂商ID（0x0277）
            if (len >= 3 && payload[0] == 0x77 && payload[1] == 0x02) //{0x77, 0x02};  厂商ID（低字节在前）
            {
                printf("[Vehicle State] ");
                for (int i = 2; i < len; i++) {
                    printf("%02X ", payload[i]);
                }
                if((payload[2]==0xAA)&&(payload[3]==0xA1)&&(payload[4]==0xA2)&&(payload[5]==0xA5))
                {
                    printf("rssi_average=%d dbm\r\n",rssi_average);
                    double exponent = (abs((int8_t)rssi_average) - A) / (10.0 * n); // 浮点除法
                    rssi_distance = (int)pow(10.0, exponent);                    // 幂运算
                    printf("d=%d\r\n",rssi_distance);
                    Sle_Anounce_Flag=1;
                    printf("turn right\r\n");
                    


                }

                 if((payload[2]==0xAA)&&(payload[3]==0xA1)&&(payload[4]==0xA3)&&(payload[5]==0xA5))
                {
                     printf("rssi=%d dbm\r\n",(int8_t)seek_result_data->rssi);
                    double exponent = (abs((int8_t)seek_result_data->rssi) - A) / (10.0 * n); // 浮点除法
                   rssi_distance = (int)pow(10.0, exponent);                    // 幂运算
                    printf("d=%d\r\n",rssi_distance);
                    
                   
                    Sle_Anounce_Flag=2;
                     printf("turn left\r\n");


                }

                if((payload[2]==0xAA)&&(payload[3]==0xA1)&&(payload[4]==0xA4)&&(payload[5]==0xA5))
                {
                     printf("rssi=%d dbm\r\n",(int8_t)seek_result_data->rssi);
                    double exponent = (abs((int8_t)seek_result_data->rssi) - A) / (10.0 * n); // 浮点除法
                    rssi_distance = (int)pow(10.0, exponent);                    // 幂运算
                    printf("d=%d\r\n",rssi_distance);
                    
                    
                    Sle_Anounce_Flag=3;
                     printf("car overtake\r\n");


                }

                if((payload[2]==0xAA)&&(payload[3]==0xA1)&&(payload[4]==0xA5)&&(payload[5]==0xA5))   //求救信息包含定位和紧急电话通知
                {
                     printf("rssi=%d dbm\r\n",(int8_t)seek_result_data->rssi);
                    double exponent = (abs((int8_t)seek_result_data->rssi) - A) / (10.0 * n); // 浮点除法
                    rssi_distance = (int)pow(10.0, exponent);                    // 幂运算
                    printf("d=%d\r\n",rssi_distance);
                  
                    
                    Sle_Anounce_Flag=4;
                     printf("sos help\r\n");


                }
                printf("\n");
            }
        }
        idx += len - 1;  // 移动到下一个AD结构
    }

    if (g_sle_uart_conn_num < SLE_UART_CLIENT_MAX_CON) {
        if (strstr((const char *)seek_result_data->data, SLE_SERVER_NAME) != NULL) {
            printf("will connect dev\n");
            memcpy_s(&g_sle_remote_addr, sizeof(sle_addr_t), 
                     &seek_result_data->addr, sizeof(sle_addr_t));// 扫描到目标设备，将目标设备的地址拷贝到g_sle_remote_addr
            sle_stop_seek();                                           // 停止扫描
        }
    }
}



/* 扫描关闭回调函数 */
/*发起连接请求，连接成功后触发连接状态改变回调函数*/
static void sle_client_seek_disable_cbk(errcode_t status)
{
    if (status != 0) {
        printf("[%s] status error = %x\r\n", __FUNCTION__, status);
    } else {
        sle_connect_remote_device(&g_sle_remote_addr);       // 发送连接请求
    }
}
/* 扫描注册初始化函数 */
static void sle_client_seek_cbk_register(void)
{
    g_sle_uart_seek_cbk.sle_enable_cb = sle_client_sle_enable_cbk;
    g_sle_uart_seek_cbk.seek_enable_cb = sle_client_seek_enable_cbk;
    g_sle_uart_seek_cbk.seek_result_cb = sle_client_seek_result_info_cbk;
    g_sle_uart_seek_cbk.seek_disable_cb = sle_client_seek_disable_cbk;
    sle_announce_seek_register_callbacks(&g_sle_uart_seek_cbk);
}
/* 连接状态改变回调 */
/*物理连接后开始配对连接，完成后触发配对完成回调函数*/
static void sle_client_connect_state_changed_cbk(uint16_t conn_id,
                                                 const sle_addr_t *addr,
                                                 sle_acb_state_t conn_state,
                                                 sle_pair_state_t pair_state,
                                                 sle_disc_reason_t disc_reason)
{
    unused(addr);//消除编译器中关于某些变量未使用的警告
    unused(pair_state);
    printf("[%s] disc_reason:0x%x\r\n", __FUNCTION__, disc_reason);

    if (conn_state == SLE_ACB_STATE_CONNECTED) {
        printf(" SLE_ACB_STATE_CONNECTED\r\n");
        g_sle_con_id[g_sle_uart_conn_num] = conn_id;//保存连接ID
        ssap_exchange_info_t info = {0};
        info.mtu_size = SLE_MTU_SIZE_DEFAULT;
        info.version = 1;
        ssapc_exchange_info_req(1, g_sle_con_id[g_sle_uart_conn_num], &info);
        g_sle_uart_conn_num++;

        
        printf(" sle_low_latency_rx_enable \r\n");
    } else if (conn_state == SLE_ACB_STATE_NONE) {
        printf(" SLE_ACB_STATE_NONE\r\n");
    } else if (conn_state == SLE_ACB_STATE_DISCONNECTED) {
        printf(" SLE_ACB_STATE_DISCONNECTED\r\n");
        
        g_sle_uart_conn_num--;
        sle_start_scan();
    } else {
        printf(" status error\r\n");
    }
    sle_start_seek();
}
/* 配对完成回调 */
void sle_client_pair_complete_cbk(uint16_t conn_id, const sle_addr_t *addr, errcode_t status)
{
    printf("[%s] conn_id:%d, state:%d,addr:%02x***%02x%02x\n", __FUNCTION__, conn_id, status, addr->addr[0],
           addr->addr[4], addr->addr[5]); /* 0 14 15: addr */
    if (status == 0) {
        ssap_exchange_info_t info = {0};
        info.mtu_size = SLE_MTU_SIZE_DEFAULT;
        info.version = 1;
        ssapc_exchange_info_req(0, g_sle_con_id[g_sle_uart_conn_num], &info);//发送MTU请求,触发更新mtu回调
        //ssapc_exchange_info_req(0, g_sle_uart_conn_id, &info);
    }
}
/* 连接注册初始化函数 */
static void sle_client_connect_cbk_register(void)
{
    g_sle_uart_connect_cbk.connect_state_changed_cb = sle_client_connect_state_changed_cbk;
    g_sle_uart_connect_cbk.pair_complete_cb = sle_client_pair_complete_cbk;
    sle_connection_register_callbacks(&g_sle_uart_connect_cbk);
}
/* 更新mtu回调 */
static void sle_client_exchange_info_cbk(uint8_t client_id,
                                         uint16_t conn_id,
                                         ssap_exchange_info_t *param,
                                         errcode_t status)
{
    printf("[%s] pair complete client id:%d status:%d\r\n", __FUNCTION__, client_id, status);
    printf("[%s] mtu size: %d, version: %d.\r\n", __FUNCTION__, param->mtu_size, param->version);
    ssapc_find_structure_param_t find_param = {0};
    find_param.type = SSAP_FIND_TYPE_PROPERTY;//查找类型为属性，SSAP_FIND_TYPE_PROPERTY用于标识客户端在服务发现过程中需要查找的目标类型为服务属性。这包括特征值（Characteristic）、描述符（Descriptor）等数据端点
    find_param.start_hdl = 1;//1,0xFFFF覆盖整个句柄空间，确保不遗漏任何潜在属性
    find_param.end_hdl = 0xFFFF;
    ssapc_find_structure(0, conn_id, &find_param);//根据设置的查找参数，协议栈会向服务端发送属性查询请求。服务端响应包含匹配的属性列表，客户端通过发现特征回调函数接收结果
}

/* 发现服务回调 */  
/*在更新mtu回调中的ssapc_find_structure(0, conn_id, &find_param)，因为设置为查找属性，而非查找服务，所以不会触发服务回调函数*/
static void sle_client_find_structure_cbk(uint8_t client_id,
                                          uint16_t conn_id,
                                          ssapc_find_service_result_t *service,
                                          errcode_t status)
{
    printf("[%s] client: %d conn_id:%d status: %d \r\n", __FUNCTION__, client_id, conn_id, status);
    printf("[%s] find structure start_hdl:[0x%02x], end_hdl:[0x%02x], uuid len:%d\r\n", __FUNCTION__,
           service->start_hdl, service->end_hdl, service->uuid.len);
    g_sle_find_service_result.start_hdl = service->start_hdl;
    g_sle_find_service_result.end_hdl = service->end_hdl;
    memcpy_s(&g_sle_find_service_result.uuid, sizeof(sle_uuid_t), &service->uuid, sizeof(sle_uuid_t));
}

/* 发现特征回调 */
/*property参数是由协议栈内部填充后传递给用户的，用户代码仅需读取其内容而非初始化输入。，所以其为输出型指针*/
static void sle_client_find_property_cbk(uint8_t client_id,
                                         uint16_t conn_id,
                                         ssapc_find_property_result_t *property,
                                         errcode_t status)
{
    printf(
        "[%s] client id: %d, conn id: %d, operate ind: %d, "
        "descriptors count: %d status:%d property->handle %d\r\n",
        __FUNCTION__, client_id, conn_id, property->operate_indication, property->descriptors_count, status,
        property->handle);
    g_sle_send_param.handle = property->handle;//​保存当前发现特征值的操作句柄，为后续通过 SLE 协议向该特征写入数据提供关键标识
    g_sle_send_param.type = SSAP_PROPERTY_TYPE_VALUE;//类型为特征值
}
/* 发现特征完成回调 */
static void sle_client_find_structure_cmp_cbk(uint8_t client_id,
                                              uint16_t conn_id,
                                              ssapc_find_structure_result_t *structure_result,
                                              errcode_t status)
{
    unused(conn_id);
    printf("[%s] client id:%d status:%d type:%d uuid len:%d \r\n", __FUNCTION__, client_id, status,
           structure_result->type, structure_result->uuid.len);
}
/* 收到写响应回调 */
static void sle_client_write_cfm_cbk(uint8_t client_id,
                                     uint16_t conn_id,
                                     ssapc_write_result_t *write_result,
                                     errcode_t status)
{
    printf("[%s] conn_id:%d client id:%d status:%d handle:%02x type:%02x\r\n", __FUNCTION__, conn_id, client_id, status,
           write_result->handle, write_result->type);
}
/* 收到通知回调 */
void sle_notification_cbk(uint8_t client_id, uint16_t conn_id, ssapc_handle_value_t *data, errcode_t status)
{
    unused(client_id);
    unused(conn_id);
    unused(status);
    //printf("recv len:%d data: ", data->data_len);
    for (uint16_t i = 0; i < data->data_len; i++) {
        printf("%c", data->data[i]);
        
    }
    

    if (data && data->data && data->data_len >= 1) 
    {
        
   // 复制数据到缓冲区并添加终止符
    uint16_t data_len = data->data_len;
    uint8_t data_buffer[256]; // 确保足够大
    if (data_len < sizeof(data_buffer)) {
        memcpy(data_buffer, data->data, data_len);
        data_buffer[data_len] = '\0'; // 字符串终止符
    } 
    

   for(int i=0;i<6;i++)
   {
     leida_data[i]=data->data[i];

   }
     
      if((leida_data[0]==0x12)&&(leida_data[5]==0x13))
      {
         row=leida_data[1];
         distance=leida_data[2];
         speeed=leida_data[3];
         printf("zxm");

      }
      if(row>0)
         {
            flag=6;
         }
         if(row<0)
         {
            flag=7;
         }

      printf("row=%d\r\n",row);
      printf("distance=%d\r\n",distance);
      printf("speeed=%d\r\n",speeed);
    // 十六进制值比较：
    if (data->data[0] == 0x06)//允许变道
    {
        flag = 1;
        data->data[0]=0;
    }

    if (data->data[0] == 0x07)//不允许变道
    {
        flag = 2;
        data->data[0]=0;
    }



    if (data->data[0] == 0x10)//mpu6050
    {
        flag = 3;
        data->data[0]=0;
    }

    /*无钥匙*/
     if ((data->data[0] == 0x11)&&(LOCK_state==0))//解锁
    {
        
         flag = 4;
         printf("no lock");
         data->data[0]=0;
         LOCK_state=1;
        
        
    }

    if ((data->data[0] == 0x12)&&LOCK_state==1)//上锁
    {
       

         flag = 5;
         printf("lock");
         data->data[0]=0;
         LOCK_state=0;
       
        
    }

   }
    
    printf("\r\n");

}
/* 收到指示回调 */
void sle_indication_cbk(uint8_t client_id, uint16_t conn_id, ssapc_handle_value_t *data, errcode_t status)
{
    unused(client_id);
    unused(conn_id);
    unused(status);
    printf("recv len:%d data: ", data->data_len);
    for (uint16_t i = 0; i < data->data_len; i++) {
        printf("%c", data->data[i]);
    }
    printf("\r\n");
}

static void sle_client_ssapc_cbk_register(void)
{
    g_sle_uart_ssapc_cbk.exchange_info_cb = sle_client_exchange_info_cbk;
    g_sle_uart_ssapc_cbk.find_structure_cb = sle_client_find_structure_cbk;
    g_sle_uart_ssapc_cbk.ssapc_find_property_cbk = sle_client_find_property_cbk;
    g_sle_uart_ssapc_cbk.find_structure_cmp_cb = sle_client_find_structure_cmp_cbk;
    g_sle_uart_ssapc_cbk.write_cfm_cb = sle_client_write_cfm_cbk;
    g_sle_uart_ssapc_cbk.notification_cb = sle_notification_cbk;
    g_sle_uart_ssapc_cbk.indication_cb = sle_indication_cbk;
    ssapc_register_callbacks(&g_sle_uart_ssapc_cbk);
}

void sle_client_init(void)
{
    osal_msleep(500); /* 500: 延时 */
    printf("sle enable.\r\n");
    sle_client_seek_cbk_register();
    sle_client_connect_cbk_register();
    sle_client_ssapc_cbk_register();//注册SSAP客户端回调函数
    if (enable_sle() != ERRCODE_SUCC) {//使能SLE后，触发使能回调函数
        printf("sle enbale fail !\r\n");
    }
}


